#pragma once

#include <array>
#include <iostream>
#include <glm/common.hpp>
#include <glm/gtx/euler_angles.hpp>
#include <glm/glm.hpp>
#include <glm/gtc/matrix_transform.hpp>
#include <glm/gtx/matrix_decompose.hpp> 
#include <glm/gtx/norm.hpp>

#include <fstream>
#include <ostream>
#include <istream>
#include <vector>
#include <queue>
#include <map>
#include <list>
#include <algorithm>


#include "Transform.h"
#include "Camera.h"
#include "OrthoCamera.h"
#include "Plane.h"

namespace Math
{

	struct FrustumPlane
	{
		Plane topFace;
		Plane bottomFace;

		Plane rightFace;
		Plane leftFace;

		Plane farFace;
		Plane nearFace;
	};

	struct BoundingVolume
	{
		virtual bool isOnFrustumPlane(const FrustumPlane& camFrustumPlane, const Transform& transform) const = 0;

		virtual bool isOnOrForwardPlane(const Plane& plane) const = 0;

		bool isOnFrustumPlane(const FrustumPlane& camFrustumPlane) const
		{
			return (isOnOrForwardPlane(camFrustumPlane.leftFace) &&
				isOnOrForwardPlane(camFrustumPlane.rightFace) &&
				isOnOrForwardPlane(camFrustumPlane.topFace) &&
				isOnOrForwardPlane(camFrustumPlane.bottomFace) &&
				isOnOrForwardPlane(camFrustumPlane.nearFace) &&
				isOnOrForwardPlane(camFrustumPlane.farFace));
		};
	};

	struct Sphere : public BoundingVolume
	{
		glm::vec3 center{ 0.f, 0.f, 0.f };
		float radius{ 0.f };

		Sphere(const glm::vec3& inCenter, float inRadius)
			: BoundingVolume{}, center{ inCenter }, radius{ inRadius }
		{}


		bool Apart(const Sphere& other)
		{
			float distQ2 = glm::dot(center - other.center, center - other.center);
			float combinedRadius = radius + other.radius;
			float combinedRadiusSquared = combinedRadius * combinedRadius;
			
			return distQ2 > combinedRadiusSquared;

		}
	
		bool isOnOrForwardPlane(const Plane& plane) const final
		{
			return plane.getSignedDistanceToPlane(center) > -radius;
		}

		bool isOnFrustumPlane(const FrustumPlane& camFrustumPlane, const Transform& transform) const final
		{
			//Get global scale thanks to our transform
			const glm::vec3 globalScale = transform.scale;

			//Get our global center with process it with the global model matrix of our transform
			const glm::vec3 globalCenter{ glm::vec4(transform.position, 1.f) };

			//To wrap correctly our shape, we need the maximum scale scalar.
			const float maxScale = glm::max(glm::max(globalScale.x, globalScale.y), globalScale.z);

			//Max scale is assuming for the diameter. So, we need the half to apply it to our radius
			Sphere globalSphere(globalCenter, radius * (maxScale * 0.5f));

			//Check Firstly the result that have the most chance to failure to avoid to call all functions.
			bool b1 = (globalSphere.isOnOrForwardPlane(camFrustumPlane.leftFace));
			bool b2 = globalSphere.isOnOrForwardPlane(camFrustumPlane.rightFace);
			bool b3 = globalSphere.isOnOrForwardPlane(camFrustumPlane.farFace);
			bool b4 = globalSphere.isOnOrForwardPlane(camFrustumPlane.nearFace);
			bool b5 = globalSphere.isOnOrForwardPlane(camFrustumPlane.topFace);
			bool b6 = globalSphere.isOnOrForwardPlane(camFrustumPlane.bottomFace);
			//printf("%d %d %d %d %d %d \n", b1, b2, b3, b4, b5, b6);

			return b1 && b2 && b3 && b4 && b5 && b6;
		};

	};

	struct SquareAABB : public BoundingVolume
	{
		glm::vec3 center{ 0.f, 0.f, 0.f };
		float extent{ 0.f };

		SquareAABB(const glm::vec3& inCenter, float inExtent)
			: BoundingVolume{}, center{ inCenter }, extent{ inExtent }
		{}

		bool isOnOrForwardPlane(const Plane& plane) const final
		{
			// Compute the projection interval radius of b onto L(t) = b.c + t * p.n
			const float r = extent * (std::abs(plane.normal.x) + std::abs(plane.normal.y) + std::abs(plane.normal.z));
			return -r <= plane.getSignedDistanceToPlane(center);
		}

		bool isOnFrustumPlane(const FrustumPlane& camFrustumPlane, const Transform& transform) const final
		{
			//Get global scale thanks to our transform
			const glm::vec3 globalCenter{ transform.GetWorld()* glm::vec4(center, 1.f) };

			// Scaled orientation
			const glm::vec3 right = transform.GetRight() * extent;
			const glm::vec3 up = transform.GetUp() * extent;
			const glm::vec3 forward = transform.GetForward() * extent;

			const float newIi = std::abs(glm::dot(glm::vec3{ 1.f, 0.f, 0.f }, right)) +
				std::abs(glm::dot(glm::vec3{ 1.f, 0.f, 0.f }, up)) +
				std::abs(glm::dot(glm::vec3{ 1.f, 0.f, 0.f }, forward));

			const float newIj = std::abs(glm::dot(glm::vec3{ 0.f, 1.f, 0.f }, right)) +
				std::abs(glm::dot(glm::vec3{ 0.f, 1.f, 0.f }, up)) +
				std::abs(glm::dot(glm::vec3{ 0.f, 1.f, 0.f }, forward));

			const float newIk = std::abs(glm::dot(glm::vec3{ 0.f, 0.f, 1.f }, right)) +
				std::abs(glm::dot(glm::vec3{ 0.f, 0.f, 1.f }, up)) +
				std::abs(glm::dot(glm::vec3{ 0.f, 0.f, 1.f }, forward));

			const SquareAABB globalAABB(globalCenter, glm::max(glm::max(newIi, newIj), newIk));

			return (globalAABB.isOnOrForwardPlane(camFrustumPlane.leftFace) &&
				globalAABB.isOnOrForwardPlane(camFrustumPlane.rightFace) &&
				globalAABB.isOnOrForwardPlane(camFrustumPlane.topFace) &&
				globalAABB.isOnOrForwardPlane(camFrustumPlane.bottomFace) &&
				globalAABB.isOnOrForwardPlane(camFrustumPlane.nearFace) &&
				globalAABB.isOnOrForwardPlane(camFrustumPlane.farFace));
		};
	};

	struct AABB : public BoundingVolume
	{
		glm::vec3 center{ 0.f, 0.f, 0.f };
		glm::vec3 extents{ 1.f, 1.f, 1.f };

		AABB() = default;

		AABB(const glm::vec3& min, const glm::vec3& max)
			: BoundingVolume{}, center{ (max + min) * 0.5f }, extents{ max.x - center.x, max.y - center.y, max.z - center.z }
		{}

		AABB(const glm::vec3& inCenter, float iI, float iJ, float iK)
			: BoundingVolume{}, center{ inCenter }, extents{ iI, iJ, iK }
		{}

		std::array<glm::vec3, 8> getVertice() const
		{
			std::array<glm::vec3, 8> vertice;
			vertice[0] = { center.x - extents.x, center.y - extents.y, center.z - extents.z };
			vertice[1] = { center.x + extents.x, center.y - extents.y, center.z - extents.z };
			vertice[2] = { center.x - extents.x, center.y + extents.y, center.z - extents.z };
			vertice[3] = { center.x + extents.x, center.y + extents.y, center.z - extents.z };
			vertice[4] = { center.x - extents.x, center.y - extents.y, center.z + extents.z };
			vertice[5] = { center.x + extents.x, center.y - extents.y, center.z + extents.z };
			vertice[6] = { center.x - extents.x, center.y + extents.y, center.z + extents.z };
			vertice[7] = { center.x + extents.x, center.y + extents.y, center.z + extents.z };
			return vertice;
		}

		//see https://gdbooks.gitbooks.io/3dcollisions/content/Chapter2/static_aabb_plane.html
		bool isOnOrForwardPlane(const Plane& plane) const final
		{
			// Compute the projection interval radius of b onto L(t) = b.c + t * p.n
			const float r = extents.x * std::abs(plane.normal.x) + extents.y * std::abs(plane.normal.y) +
				extents.z * std::abs(plane.normal.z);

			return -r <= plane.getSignedDistanceToPlane(center);
		}

		bool isOnFrustumPlane(const FrustumPlane& camFrustum, const Transform& transform) const final
		{
			//Get global scale thanks to our transform
			const glm::vec3 globalCenter{ transform.GetWorld()* glm::vec4(center, 1.f) };

			// Scaled orientation
			const glm::vec3 right = transform.GetRight() * extents.x;
			const glm::vec3 up = transform.GetUp() * extents.y;
			const glm::vec3 forward = transform.GetForward() * extents.z;

			const float newIi = std::abs(glm::dot(glm::vec3{ 1.f, 0.f, 0.f }, right)) +
				std::abs(glm::dot(glm::vec3{ 1.f, 0.f, 0.f }, up)) +
				std::abs(glm::dot(glm::vec3{ 1.f, 0.f, 0.f }, forward));

			const float newIj = std::abs(glm::dot(glm::vec3{ 0.f, 1.f, 0.f }, right)) +
				std::abs(glm::dot(glm::vec3{ 0.f, 1.f, 0.f }, up)) +
				std::abs(glm::dot(glm::vec3{ 0.f, 1.f, 0.f }, forward));

			const float newIk = std::abs(glm::dot(glm::vec3{ 0.f, 0.f, 1.f }, right)) +
				std::abs(glm::dot(glm::vec3{ 0.f, 0.f, 1.f }, up)) +
				std::abs(glm::dot(glm::vec3{ 0.f, 0.f, 1.f }, forward));

			const AABB globalAABB(globalCenter, newIi, newIj, newIk);

			bool b1 = globalAABB.isOnOrForwardPlane(camFrustum.leftFace);
			bool b2 = globalAABB.isOnOrForwardPlane(camFrustum.rightFace);
			bool b3 = globalAABB.isOnOrForwardPlane(camFrustum.topFace);
			bool b4 = globalAABB.isOnOrForwardPlane(camFrustum.bottomFace);
			bool b5 = globalAABB.isOnOrForwardPlane(camFrustum.nearFace);
			bool b6 = globalAABB.isOnOrForwardPlane(camFrustum.farFace);

			return b1 && b2 && b3 && b4 && b5 && b6;
		};
	};

	inline FrustumPlane createFrustumPlaneFromCamera(const Transform& transform, const Frustum& frustum)
	{
		const float aspect = frustum.aspect;
		const float fovY = glm::radians(frustum.fovy);
		const float zNear = frustum.nNear;
		const float zFar = frustum.nFar;

		const float halfVSide = zFar * tanf(fovY * .5f);
		const float halfHSide = halfVSide * aspect;


		const glm::vec3 camPos = transform.position;
		const glm::vec3 right = transform.GetRight();
		const glm::vec3 front = transform.GetForward();
		const glm::vec3 camUp = transform.GetUp();
		const glm::vec3 frontMultNear = zNear * front;;
		const glm::vec3 frontMultFar = zFar * front;


		FrustumPlane  frustumPlane;

		frustumPlane.leftFace = { camPos, -glm::cross(camUp, frontMultFar + right * halfHSide) };
		frustumPlane.rightFace = { camPos, -glm::cross(frontMultFar - right * halfHSide, camUp) };
		frustumPlane.topFace = { camPos, -glm::cross(right, frontMultFar - camUp * halfVSide) };
		frustumPlane.bottomFace = { camPos, -glm::cross(frontMultFar + camUp * halfVSide, right) };
		frustumPlane.nearFace = { camPos + frontMultNear, front };
		frustumPlane.farFace = { camPos + frontMultFar, -front };
		return frustumPlane;
	}

	inline FrustumPlane createViewportPlaneFromCamera(const Transform& transform, const Viewport& vp)
	{

		const float halfVSide = (vp.top - vp.bottom) / 2.0f;
		const float halfHSide = (vp.right - vp.left) / 2.0f;
		const float zNear = vp.zNear;
		const float zFar = vp.zFar;

		const glm::vec3 camPos= transform.position;
		const glm::vec3 right = transform.GetRight();
		const glm::vec3 front = transform.GetForward();
		const glm::vec3 camUp = transform.GetUp();
		const glm::vec3 frontMultNear = zNear * front;;
		const glm::vec3 frontMultFar = zFar * front;

		const glm::vec3 nearRightTop = camPos + frontMultNear + halfHSide * right + halfVSide * camUp;
		const glm::vec3 farLeftBottom = camPos + frontMultFar - halfHSide * right - halfVSide * camUp;


		FrustumPlane frustumPlane;

		frustumPlane.leftFace = { farLeftBottom, right };
		frustumPlane.rightFace = { nearRightTop, -right };
		frustumPlane.farFace = { farLeftBottom, -front };
		frustumPlane.nearFace = { nearRightTop, front };
		frustumPlane.topFace = { nearRightTop ,  -camUp };
		frustumPlane.bottomFace = { farLeftBottom, camUp };

		return frustumPlane;
	}
}
